#include "nlohmann/json.hpp"
#include <sys/time.h>
#include <chrono>
#include <memory>
#include <errno.h>
#include <fcntl.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <termios.h>
#include <iomanip>
#include <unistd.h>
#include <iostream>
#include <fstream> // 读写文件的头文件
#include <ctime>
#include <math.h>
#include <list>
#include <opencv2/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "std_msgs/msg/u_int8_multi_array.hpp"
#include "std_msgs/msg/int32_multi_array.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"

// #include "image_transport/image_transport/image_transport.hpp"
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/msg/image.hpp>

#include <iostream>
#include <cstring>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <fcntl.h>

#include "../../include/Typedef.h"
#include "../../include/boat_datadef.h"
#include "../motion_plan/motion_plan.h"
#include "../controller/motion_control.h"


using namespace cv;
using json = nlohmann::json;
using namespace std::chrono_literals;

bool track_flash_flag = false, formation_flash_flag = false;

flash_model_data taskFlashModelData;

anchor_mission_control taskAnchorMissionControl;

actuator_control actionActuatorControl;
trajectory_mission_control actionTrajectoryMissionControl;

kinematic_state usvposture;


bool stateFlashFlag = false;

MotionPlan motionPlanner;
TransState trans_state;
MotionControl controller;
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher_plan;
bool action_send_flag = false;
char getstatenum = 0, runstatenum = 0;

Mat local_map(800, 800, CV_8UC3, Scalar(0, 0, 0));
vector<vector<int>> premap(800, vector<int>(800));
bool mapshowflag = false;

double get_wall_time() // 单位s
{
    struct timeval time;
    if (gettimeofday(&time, NULL))
    {
        return 0;
    }
    return (double)time.tv_sec + (double)time.tv_usec * .000001;
}

void ControlStateCallback(std_msgs::msg::Float64MultiArray::SharedPtr msg)
{
    getstatenum++;
    stateFlashFlag = true;
    usvposture.kind = msg->data[0];
    usvposture.num = msg->data[1];

    usvposture.roll = msg->data[2];       /*roll angle, deg*/
    usvposture.pitch = msg->data[3];      /*pitch angle, deg*/
    usvposture.yaw = msg->data[4];        /*yaw angle, deg*/
    usvposture.rollspeed = msg->data[5];  /*deg/s*/
    usvposture.pitchspeed = msg->data[6]; /*deg/s*/
    usvposture.yawspeed = msg->data[7];   /*deg/s*/

    usvposture.lat = msg->data[8];   /*x, m*/
    usvposture.lon = msg->data[9];   /*y, m*/
    usvposture.alt = msg->data[10];  /*z, m*/
    usvposture.vx = msg->data[11];   /*< [m/s] Ground X Speed (Latitude)*/
    usvposture.vy = msg->data[12];   /*< [m/s] Ground Y Speed (Longitude)*/
    usvposture.vz = msg->data[13];   /*< [m/s] Ground Z Speed (Altitude)*/
    usvposture.xacc = msg->data[14]; /*< [G] X acceleration*/
    usvposture.yacc = msg->data[15]; /*< [G] Y acceleration*/
    usvposture.zacc = msg->data[16]; /*< [G] Z acceleration*/

    bool show_flag = false;
    if (action_send_flag)
    {
        std_msgs::msg::Float64MultiArray plan_msg;
        plan_msg.data.clear();
        if (show_flag)
        {
            std::cout << "/**************Control output***************" << std::endl;
        }
        if (motionPlanner.actionModelData.model == 0)
        {
            plan_msg.data.push_back(motionPlanner.actionModelData.model);
            plan_msg.data.push_back(motionPlanner.actionActuatorControl.steer_l);
            plan_msg.data.push_back(motionPlanner.actionActuatorControl.steer_r);
            plan_msg.data.push_back(motionPlanner.actionActuatorControl.motor_l);
            plan_msg.data.push_back(motionPlanner.actionActuatorControl.motor_r);
            plan_msg.data.push_back(motionPlanner.actionActuatorControl.bucket_l);
            plan_msg.data.push_back(motionPlanner.actionActuatorControl.bucket_r);
            if (show_flag)
            {
                std::cout << "action model:" << (float)motionPlanner.actionModelData.model << ", steer_l:" << motionPlanner.actionActuatorControl.steer_l << ", steer_r:" << motionPlanner.actionActuatorControl.steer_r
                            << ", motor_l:" << motionPlanner.actionActuatorControl.motor_l << ", motor_r:" << motionPlanner.actionActuatorControl.motor_r
                            << ", bucket_l:" << motionPlanner.actionActuatorControl.bucket_l << ", bucket_r:" << motionPlanner.actionActuatorControl.bucket_r << std::endl;
            }
        }
        else if (motionPlanner.actionModelData.model == 10)
        {
            runstatenum++;
            actuator_control preaction = controller.USVControl(motionPlanner.actionHeadvelControl, trans_state);

            plan_msg.data.push_back(0);
            plan_msg.data.push_back(preaction.steer_l);
            plan_msg.data.push_back(preaction.steer_r);
            plan_msg.data.push_back(preaction.motor_l);
            plan_msg.data.push_back(preaction.motor_r);
            plan_msg.data.push_back(preaction.bucket_l);
            plan_msg.data.push_back(preaction.bucket_r);

            if (show_flag)
            {
                std::cout << "action model:" << (float)motionPlanner.actionModelData.model << ", model:" << (float)motionPlanner.actionHeadvelControl.model << ", heading:" << motionPlanner.actionHeadvelControl.heading << ", velocity:" << motionPlanner.actionHeadvelControl.velocity << std::endl;
                std::cout << "steer_l:" << preaction.steer_l << ", steer_r:" << preaction.steer_r
                            << ", motor_l:" << preaction.motor_l << ", motor_r:" << preaction.motor_r
                            << ", bucket_l:" << preaction.bucket_l << ", bucket_r:" << preaction.bucket_r << std::endl;
            }
        }
        else
        {
            plan_msg.data.push_back(0);
            plan_msg.data.push_back(0);
            plan_msg.data.push_back(0);
            if (show_flag)
            {
                printf("Unknown Control Action! \n");
            }
        }
        publisher_plan->publish(plan_msg);
    }
    else
    {
        if (show_flag)
        {
            printf("Don't Send Control Action! \n");
        }
    }
}

void NeiborStateCallback(std_msgs::msg::Float64MultiArray::SharedPtr msg)
{
    kinematic_state preposture;
    preposture.kind = msg->data[0];
    preposture.num = msg->data[1];

    preposture.roll = msg->data[2];       /*roll angle, deg*/
    preposture.pitch = msg->data[3];      /*pitch angle, deg*/
    preposture.yaw = msg->data[4];        /*yaw angle, deg*/
    preposture.rollspeed = msg->data[5];  /*deg/s*/
    preposture.pitchspeed = msg->data[6]; /*deg/s*/
    preposture.yawspeed = msg->data[7];   /*deg/s*/

    preposture.lat = msg->data[8];   /*x, m*/
    preposture.lon = msg->data[9];   /*y, m*/
    preposture.alt = msg->data[10];  /*z, m*/
    preposture.vx = msg->data[11];   /*< [m/s] Ground X Speed (Latitude)*/
    preposture.vy = msg->data[12];   /*< [m/s] Ground Y Speed (Longitude)*/
    preposture.vz = msg->data[13];   /*< [m/s] Ground Z Speed (Altitude)*/
    preposture.xacc = msg->data[14]; /*< [G] X acceleration*/
    preposture.yacc = msg->data[15]; /*< [G] Y acceleration*/
    preposture.zacc = msg->data[16]; /*< [G] Z acceleration*/

    /*在获取neibor数据时需要注意线程同步*/

    // std::cout << "neibor kind:" << preposture.kind << " num:" << preposture.num<< " x:" << preposture.lat  << std::endl;

}


void EnvironCallback(std_msgs::msg::UInt8MultiArray::SharedPtr msg)
{
    double stime, etime;
    stime = get_wall_time();

    if(mapshowflag == true)
    {
        return;
    }
    int i, j;
    int testNum = 800;
    local_map = Mat::zeros(testNum, testNum, CV_8UC3);
    unsigned int datanum = msg->data.size();

    unsigned char preseq = 0;
    unsigned char predata = 0;
    unsigned int dataseq = 0;
    unsigned char flagdata = 0x80;
    for (i = 0; i < 800; i++)
    {
        for (j = 0; j < 800; j++)
        {
            predata = msg->data[dataseq];
            preseq++;
            if((predata&flagdata)==flagdata){
                premap[i][j] = 1;
            }else{
                premap[i][j] = 0;
                local_map.at<Vec3b>(testNum - 1 - (j), i) = Vec3b(255, 255, 255);
            }
            flagdata=(flagdata>>1);
            if(preseq>=8){
                flagdata = 0x80;
                preseq = 0;
                dataseq++;
                if(dataseq >= datanum) break;
            }
        }
    }
    // printf("image data num is %d\n", dataseq);
    etime = get_wall_time();
    mapshowflag = true;
}

void* MapShow(void *arg)
{
    double start,end;
    int datanum = 0;
    start = get_wall_time();
    unsigned char period = 0;
    while(1)
    {
        if(mapshowflag){
            
            imshow("map", local_map);
            mapshowflag = false;
            waitKey(1);
        }
    }
}

int main(int argc, char** argv)
{
    // ROS2 初始化
    rclcpp::init(argc, argv);
    
    // 创建节点 - 使用启动文件提供的名称
    auto plan_node = std::make_shared<rclcpp::Node>("ship_node");
    // auto node = rclcpp::Node::make_shared("ship_node");
    
    // 获取配置参数
    std::string config_json;
    
    try {
        // 声明并获取配置参数
        plan_node->declare_parameter("ship_config", "");
        config_json = plan_node->get_parameter("ship_config").as_string();
    } catch (const rclcpp::exceptions::ParameterNotDeclaredException& e) {
        RCLCPP_ERROR(plan_node->get_logger(), "未找到 'ship_config' 参数: %s", e.what());
        return 1;
    }
    
    // 配置解析
    int usv_type = 0;
    int usv_num = 0;
    std::vector<MissionItem> missions;
    
    try {
        json config = json::parse(config_json);
        
        // 提取基本信息
        usv_type = config["type"].get<int>();
        usv_num = config["id"].get<int>();

        std::cout << "type: " << usv_type << "   id: " << usv_num << std::endl;
        
        // 提取航点
        for (const auto& wp : config["waypoints"]) {
            MissionItem point {
                .command = wp["command"].get<uint16_t>(),
                .seq = wp["seq"].get<uint8_t>(),
                .param1 = wp["param1"].get<float>(),
                .param2 = wp["param2"].get<float>(),
                .param3 = wp["param3"].get<float>(),
                .param4 = wp["param4"].get<float>(),
                .lat = wp["x"].get<float>(),
                .lon = wp["y"].get<float>(),
                .alt = wp["z"].get<float>()
            };
            missions.push_back(point);
        }
    } catch (const json::parse_error& e) {
        RCLCPP_ERROR(plan_node->get_logger(), "JSON解析错误: %s", e.what());
        return 1;
    } catch (const json::type_error& e) {
        RCLCPP_ERROR(plan_node->get_logger(), "JSON类型错误: %s", e.what());
        return 1;
    } catch (const std::exception& e) {
        RCLCPP_ERROR(plan_node->get_logger(), "配置解析失败: %s", e.what());
        return 1;
    }
    for(int i = 0; i<missions.size(); i++){
        std::cout << "seq: " << missions[i].seq*1.0 << " x: " << missions[i].lat<< " y: " << missions[i].lon<< " z: " << missions[i].alt << std::endl;
    }

    double start, end;
    double duration[10];
    int datanum = 0;
    start = get_wall_time();

    cv::namedWindow("map", WINDOW_NORMAL);


    std::string usv_state_name = "usv_state";
    usv_state_name += "_";
    usv_state_name += std::to_string(usv_type);
    usv_state_name += "_";
    usv_state_name += std::to_string(usv_num);

    std::string usv_control_name = "usv_control";
    usv_control_name += "_";
    usv_control_name += std::to_string(usv_type);
    usv_control_name += "_";
    usv_control_name += std::to_string(usv_num);

    std::string environ_topic_name = "environ";
    environ_topic_name += "_";
    environ_topic_name += std::to_string(usv_type);
    environ_topic_name += "_";
    environ_topic_name += std::to_string(usv_num);



    controller.SetController(usv_type);
    std::string neibor_name = "neibor";
    
    rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 2);  // 使用2个工作线程
    auto qos = rclcpp::QoS(rclcpp::KeepLast(10000));
    auto subscription_neibor = plan_node->create_subscription<std_msgs::msg::Float64MultiArray>(neibor_name.c_str(), qos, NeiborStateCallback); // 接收zhuagntai 指令
    
    publisher_plan = plan_node->create_publisher<std_msgs::msg::Float64MultiArray>(usv_control_name.c_str(), 10); // 发布状态信息
    auto subscription_state = plan_node->create_subscription<std_msgs::msg::Float64MultiArray>(usv_state_name.c_str(), 2, ControlStateCallback); // 接收zhuagntai 指令
    auto subscription_environ = plan_node->create_subscription<std_msgs::msg::UInt8MultiArray>(environ_topic_name.c_str(), 2, EnvironCallback);    // 接收huanjing

    executor.add_node(plan_node);

    std::thread executor_thread([&executor]() {
        RCLCPP_INFO(rclcpp::get_logger("executor"), "Starting executor thread");
        executor.spin();
        RCLCPP_INFO(rclcpp::get_logger("executor"), "Executor thread stopped");
    });

    memset(&trans_state, 0, sizeof(TransState));
    pthread_t mapshowth;
    int ret = pthread_create(&mapshowth, NULL, MapShow, NULL);
    if (ret != 0)
    {
        perror("creat map show thread failed!");
    }

    double anglevel[2];
    unsigned char shownum = 0;
    bool show_flag = false;
    bool homepos_flag = true;
    double starttime = get_wall_time();
    double endtime = 0;
    rclcpp::WallRate loop_rate(10);
    int tasktime = 0;
    while (rclcpp::ok())
    {
        if (stateFlashFlag)
        {
            tasktime++;
            if(tasktime == 10)
            {
                track_flash_flag = true;
                taskFlashModelData.model = 20;
                taskFlashModelData.flash_flag = 1;
                motionPlanner.dotTrackingParam.TrackNum = 0;
                motionPlanner.dotTrackingParam.DotNum = 0;
                if(missions.size()>512){
                    taskAnchorMissionControl.mission_num = 512;
                }else{
                    taskAnchorMissionControl.mission_num = missions.size();
                }
                for (int ii = 0; ii < taskAnchorMissionControl.mission_num; ii++)
                {
                    taskAnchorMissionControl.mission[ii] = missions[ii];
                }
            }
            stateFlashFlag = false;

            trans_state.PostoHome[0] = usvposture.lat;//lengthangle[0] * sinf(lengthangle[1] * M_PI / 180.0) / 100.0;
            trans_state.PostoHome[1] = usvposture.lon;//lengthangle[0] * cosf(lengthangle[1] * M_PI / 180.0) / 100.0;
            trans_state.PostoHome[2] = usvposture.alt;
            trans_state.PostoHome[3] = usvposture.yaw;

            trans_state.VelEarth[0] = usvposture.vx;
            trans_state.VelEarth[1] = usvposture.vy;
            trans_state.VelEarth[2] = usvposture.vz;
            trans_state.VelEarth[3] = usvposture.yaw;

            trans_state.VelPolar[0] = sqrtf(usvposture.vx * usvposture.vx + usvposture.vy * usvposture.vy);
            trans_state.VelPolar[1] = atan2(usvposture.vx, usvposture.vy) * 180 / M_PI;
            if (trans_state.VelPolar[1] < 0)
            {
                trans_state.VelPolar[1] += 360;
            }
            trans_state.VelPolar[2] = usvposture.vz;
            trans_state.VelPolar[3] = usvposture.yaw;

            shownum++;
            if (shownum >= 10)
            {
                shownum = 0;
                show_flag = true;
                int prestatenum = getstatenum;
                getstatenum = 0;
                int prerunstatenum = runstatenum;
                runstatenum = 0;
                endtime = get_wall_time();
                double runtime = endtime - starttime;
                starttime = endtime;
                
                std::cout << "/**************Control Model***************/" << std::endl;
                printf("getstatenum: %d, runstatenum: %d, runtime:%f\n", prestatenum, prerunstatenum, runtime);
                printf("x:%.9f, y: %.9f, phi:%f\n", trans_state.PostoHome[0], trans_state.PostoHome[1], trans_state.PostoHome[3]);
                std::cout << "Vel: " << trans_state.VelPolar[0] << ", VelAngle: " << trans_state.VelPolar[1] << std::endl;
                std::cout << "motionPlanner.dotTrackingParam.TrackNum: " << motionPlanner.dotTrackingParam.TrackNum << std::endl;
            }
            else
            {
                show_flag = false;
            }

            if (0 == taskFlashModelData.model)
            {
                action_send_flag = false;
                if (show_flag)
                {
                    std::cout << "Control model: No Control task" << std::endl;
                }
            }
            else if (20 == taskFlashModelData.model)
            {
                if (show_flag)
                {
                    std::cout << "Control model: Anchor Mission task" << std::endl;
                    printf("Mission Number:%d,\n", taskAnchorMissionControl.mission_num);
                    for (unsigned char i = 0; i < taskAnchorMissionControl.mission_num; i++)
                    {
                        std::cout << setprecision(10) << "com:" << taskAnchorMissionControl.mission[i].command << ", seq: " << (float)taskAnchorMissionControl.mission[i].seq << ", param1: " << taskAnchorMissionControl.mission[i].param1 << ", param2: " << taskAnchorMissionControl.mission[i].param2 << ", param3: " << taskAnchorMissionControl.mission[i].param3 << ", param4: " << taskAnchorMissionControl.mission[i].param4 << ", lat: " << taskAnchorMissionControl.mission[i].lat << ", lon: " << taskAnchorMissionControl.mission[i].lon << ", alt: " << taskAnchorMissionControl.mission[i].alt << std::endl;
                    }
                }
                if (homepos_flag)
                {
                    if (track_flash_flag)
                    {
                        track_flash_flag = false;
                        std::cout << "trajectory get" << std::endl;
                        motionPlanner.TrajecotryGet(trans_state, taskAnchorMissionControl);
                    }

                    motionPlanner.TrajectoryTrackingDirect(trans_state);
                    action_send_flag = true;
                }
                else
                {
                    action_send_flag = false;
                    if (show_flag)
                    {
                        std::cout << "Home position undetermined, please try again!!!" << std::endl;
                    }
                }
            }
            else
            {
                action_send_flag = false;
                if (show_flag)
                {
                    std::cout << "Control model: Unknown Control task" << std::endl;
                }
            }
            if (show_flag)
            {
                printf(" \n");
            }
        }

        // rclcpp::spin_some(plan_node);
        loop_rate.sleep();
    }
    rclcpp::shutdown();
    pthread_exit(&mapshowth);
    plan_node = nullptr;
    return 0;
}



